//
// Created by wangjunjun on 2024/3/25.
//

#include "messageLcmHandler.h"
#include <fstream>
#include <iostream>
std::shared_ptr<CMessageLcmHandler> CMessageLcmHandler::m_Instance= nullptr;
std::once_flag CMessageLcmHandler::m_flag;
CMessageLcmHandler::CMessageLcmHandler(){
    m_pGenptr =  std::make_shared<CGeneric>();
}
CMessageLcmHandler::~CMessageLcmHandler(){

}


std::shared_ptr<CMessageLcmHandler> CMessageLcmHandler::GetInStance() {
    std::call_once(m_flag, []() {
        m_Instance = std::shared_ptr<CMessageLcmHandler>(new CMessageLcmHandler(),
                                              [](CMessageLcmHandler* ptr){delete ptr;});
    });
    return m_Instance;
}

void  CMessageLcmHandler::Start(){
    //启动新线程执行LCM
    std::thread lcmThread(&CMessageLcmHandler::HandlerLcmProc,this);
    lcmThread.detach();
}
void CMessageLcmHandler::HandlerImuData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const micro_lcmt::microstrain_lcmt* msg){
    // 在互斥锁保护下处理接收到的IMU数据
    {
        std::lock_guard<std::mutex> lock(dataLock);
        // 解码消息中的数据
        imuData = {{msg->rpy[0], msg->rpy[1], msg->rpy[2]},
                   {msg->omega[0], msg->omega[1], msg->omega[2]},
                   {msg->acc[0], msg->acc[1], msg->acc[2]}};


    }
}
void CMessageLcmHandler::HandlerStateData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const state_lcmt::state_estimator_lcmt* msg){
    // 在互斥锁保护下处理接收到的状态估计数据
    {
        std::lock_guard<std::mutex> lock(dataLock);
        // 解码消息中的数据
        stateEstData = {{msg->p[0], msg->p[1], msg->p[2]},
                        {msg->rpy[0], msg->rpy[1], msg->rpy[2]},
                        {msg->omegaBody[0], msg->omegaBody[1], msg->omegaBody[2]},msg->power};

    }
}
void CMessageLcmHandler::HandlerLegData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const leg_control_data_lcmt* msg){
    // 在互斥锁保护下处理接收到的状态估计数据
    {
        std::lock_guard<std::mutex> lock(dataLock);
        for (int i = 0; i < 12; ++i) {
            legControlData.q[i] = msg->q[i];
            legControlData.qd[i] = msg->qd[i];
            legControlData.p[i] = msg->p[i];
            legControlData.v[i] = msg->v[i];
        }
    }
}
void CMessageLcmHandler::HandlerLegCommandData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const channel::leg_control_command_lcmt* msg){
    // 在互斥锁保护下处理接收到的状态估计数据
    {
        std::lock_guard<std::mutex> lock(dataLock);
        for (int i = 0; i < 12; ++i) {
            legControlCommand.tau_ff[i] = msg->tau_ff[i];
            legControlCommand.f_ff[i] = msg->f_ff[i];
            legControlCommand.q_des[i] = msg->q_des[i];
            legControlCommand.qd_des[i] = msg->qd_des[i];
            legControlCommand.p_des[i] = msg->p_des[i];
            legControlCommand.v_des[i] = msg->v_des[i];
            legControlCommand.kp_cartesian[i] = msg->kp_cartesian[i];
            legControlCommand.kd_cartesian[i] = msg->kd_cartesian[i];
            legControlCommand.kp_joint[i] = msg->kp_joint[i];
            legControlCommand.kd_joint[i] = msg->kd_joint[i];
        }
    }
}
void CMessageLcmHandler::HandlerUdpData(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const udp_data_lcm_t* msg){
    // 在互斥锁保护下处理接收到的状态估计数据
    {
        std::lock_guard<std::mutex> lock(dataLock);

        udpData = {{msg->p_des[0], msg->p_des[1]},{msg->v_des[0], msg->v_des[1], msg->v_des[2]},
                   {msg->rpy_des[0], msg->rpy_des[1], msg->rpy_des[2]},{msg->omega_des[0], msg->omega_des[1], msg->omega_des[2]}, msg->mode};

    }
}

void CMessageLcmHandler::HandlerLcmProc(void){
    lcm::LCM lcm;
    if (!lcm.good()) {
        return ;
    }
    // 创建一个新线程，专门用于处理LCM事件
    // std::thread lcmProcessingThread(lcmThread, std::ref(lcm));
    CMessageLcmHandler handlerObject;
    lcm.subscribe("microstrain", &CMessageLcmHandler::HandlerImuData, &handlerObject);
    lcm.subscribe("state_estimator", &CMessageLcmHandler::HandlerStateData, &handlerObject);
    lcm.subscribe("leg_control_data", &CMessageLcmHandler::HandlerLegData, &handlerObject);
    lcm.subscribe("leg_control_command", &CMessageLcmHandler::HandlerLegCommandData, &handlerObject);
    lcm.subscribe("udp_data_raw", &CMessageLcmHandler::HandlerUdpData, &handlerObject);
    while (true) {
        // 在新线程中执行LCM消息处理
        lcm.handle();
    }
}

void CMessageLcmHandler::SaveSigDataToFile(){
    std::string  sigData = m_pGenptr->GetPathFile("SIG");
//    LOG_INFO("CMessageLcmHandler::SaveSigDataToFile sigData:{}",sigData);
//    printf("m_pGenptr:%p", m_pGenptr);
    if(!sigData.empty()){
        // 将消息数据存储到本地文件
        std::string  currentTime = m_pGenptr->GetTimeStamp();
        std::string filename = sigData  + "/" + currentTime + ".txt";  // 假设生成文件名的方式
        // 将消息数据存储到本地文件
        std::ofstream outputFile(filename);
        if (outputFile.is_open()) {

            outputFile << "imuData.rpy: " << imuData.rpy[0] << ", " << imuData.rpy[1] << ", " << imuData.rpy[2] << "\n";
            outputFile << "imuData.omega: " << imuData.omega[0] << ", " << imuData.omega[1] << ", " << imuData.omega[2] << "\n";
            outputFile << "imuData.acc: " << imuData.acc[0] << ", " << imuData.acc[1] << ", " << imuData.acc[2] << "\n";

            outputFile << "stateEstData.p: " << stateEstData.p[0] << ", " << stateEstData.p[1] << stateEstData.p[2] <<"\n";
            outputFile << "stateEstData.rpy: " << stateEstData.rpy[0] << ", " << stateEstData.rpy[1] << ", " << stateEstData.rpy[2] << "\n";
            outputFile << "stateEstData.omegaBody: " << stateEstData.omegaBody[0] << ", " << stateEstData.omegaBody[1] << ", " << stateEstData.omegaBody[2] << "\n";

            outputFile << "legControlData.q: " << legControlData.q[0] << ", " << legControlData.q[1] << ", " << legControlData.q[2]<< ", " << legControlData.q[3] << ", " << legControlData.q[4] << ", " << legControlData.q[5] << ", ";
            outputFile << legControlData.q[6] << ", " << legControlData.q[7] << ", " << legControlData.q[8]<< ", " << legControlData.q[9] << ", " << legControlData.q[10] << ", " << legControlData.q[11] << "\n";
            outputFile << "legControlData.qd: " << legControlData.qd[0] << ", " << legControlData.qd[1] << ", " << legControlData.qd[2]<< ", " << legControlData.qd[3] << ", " << legControlData.qd[4] << ", " << legControlData.qd[5] << ", ";
            outputFile << legControlData.qd[6] << ", " << legControlData.qd[7] << ", " << legControlData.qd[8]<< ", " << legControlData.qd[9] << ", " << legControlData.qd[10] << ", " << legControlData.qd[11] << "\n";
            outputFile << "legControlData.p: " << legControlData.p[0] << ", " << legControlData.p[1] << ", " << legControlData.p[2]<< ", " << legControlData.p[3] << ", " << legControlData.p[4] << ", " << legControlData.p[5] << ", ";
            outputFile << legControlData.p[6] << ", " << legControlData.p[7] << ", " << legControlData.p[8]<< ", " << legControlData.p[9] << ", " << legControlData.p[10] << ", " << legControlData.p[11] << "\n";
            outputFile << "legControlData.v: " << legControlData.v[0] << ", " << legControlData.v[1] << ", " << legControlData.v[2]<< ", " << legControlData.v[3] << ", " << legControlData.v[4] << ", " << legControlData.v[5] << ", ";
            outputFile << legControlData.v[6] << ", " << legControlData.v[7] << ", " << legControlData.v[8]<< ", " << legControlData.v[9] << ", " << legControlData.v[10] << ", " << legControlData.v[11] << "\n";

            outputFile << "legControlCommand.tau_ff: " << legControlCommand.tau_ff[0] << ", " << legControlCommand.tau_ff[1] << ", " << legControlCommand.tau_ff[2]<< ", " << legControlCommand.tau_ff[3] << ", " << legControlCommand.tau_ff[4] << ", " << legControlCommand.tau_ff[5] << ", ";
            outputFile << legControlCommand.tau_ff[6] << ", " << legControlCommand.tau_ff[7] << ", " << legControlCommand.tau_ff[8]<< ", " << legControlCommand.tau_ff[9] << ", " << legControlCommand.tau_ff[10] << ", " << legControlCommand.tau_ff[11] << "\n";
            outputFile << "legControlCommand.f_ff: " << legControlCommand.f_ff[0] << ", " << legControlCommand.f_ff[1] << ", " << legControlCommand.f_ff[2]<< ", " << legControlCommand.f_ff[3] << ", " << legControlCommand.f_ff[4] << ", " << legControlCommand.f_ff[5] << ", ";
            outputFile << legControlCommand.f_ff[6] << ", " << legControlCommand.f_ff[7] << ", " << legControlCommand.f_ff[8]<< ", " << legControlCommand.f_ff[9] << ", " << legControlCommand.f_ff[10] << ", " << legControlCommand.f_ff[11] << "\n";
            outputFile << "legControlCommand.q_des: " << legControlCommand.q_des[0] << ", " << legControlCommand.q_des[1] << ", " << legControlCommand.q_des[2]<< ", " << legControlCommand.q_des[3] << ", " << legControlCommand.q_des[4] << ", " << legControlCommand.q_des[5] << ", ";
            outputFile << legControlCommand.q_des[6] << ", " << legControlCommand.q_des[7] << ", " << legControlCommand.q_des[8]<< ", " << legControlCommand.q_des[9] << ", " << legControlCommand.q_des[10] << ", " << legControlCommand.q_des[11] << "\n";
            outputFile << "legControlCommand.qd_des: " << legControlCommand.qd_des[0] << ", " << legControlCommand.qd_des[1] << ", " << legControlCommand.qd_des[2]<< ", " << legControlCommand.qd_des[3] << ", " << legControlCommand.qd_des[4] << ", " << legControlCommand.qd_des[5] << ", ";
            outputFile << legControlCommand.qd_des[6] << ", " << legControlCommand.qd_des[7] << ", " << legControlCommand.qd_des[8]<< ", " << legControlCommand.qd_des[9] << ", " << legControlCommand.qd_des[10] << ", " << legControlCommand.qd_des[11] << "\n";
            outputFile << "legControlCommand.p_des: " << legControlCommand.p_des[0] << ", " << legControlCommand.p_des[1] << ", " << legControlCommand.p_des[2]<< ", " << legControlCommand.p_des[3] << ", " << legControlCommand.p_des[4] << ", " << legControlCommand.p_des[5] << ", ";
            outputFile << legControlCommand.p_des[6] << ", " << legControlCommand.p_des[7] << ", " << legControlCommand.p_des[8]<< ", " << legControlCommand.p_des[9] << ", " << legControlCommand.p_des[10] << ", " << legControlCommand.p_des[11] << "\n";
            outputFile << "legControlCommand.v_des: " << legControlCommand.v_des[0] << ", " << legControlCommand.v_des[1] << ", " << legControlCommand.v_des[2]<< ", " << legControlCommand.v_des[3] << ", " << legControlCommand.v_des[4] << ", " << legControlCommand.v_des[5] << ", ";
            outputFile << legControlCommand.v_des[6] << ", " << legControlCommand.v_des[7] << ", " << legControlCommand.v_des[8]<< ", " << legControlCommand.v_des[9] << ", " << legControlCommand.v_des[10] << ", " << legControlCommand.v_des[11] << "\n";
            outputFile << "legControlCommand.kp_cartesian: " << legControlCommand.kp_cartesian[0] << ", " << legControlCommand.kp_cartesian[1] << ", " << legControlCommand.kp_cartesian[2]<< ", " << legControlCommand.kp_cartesian[3] << ", " << legControlCommand.kp_cartesian[4] << ", " << legControlCommand.kp_cartesian[5] << ", ";
            outputFile << legControlCommand.kp_cartesian[6] << ", " << legControlCommand.kp_cartesian[7] << ", " << legControlCommand.kp_cartesian[8]<< ", " << legControlCommand.kp_cartesian[9] << ", " << legControlCommand.kp_cartesian[10] << ", " << legControlCommand.kp_cartesian[11] << "\n";
            outputFile << "legControlCommand.kd_cartesian: " << legControlCommand.kd_cartesian[0] << ", " << legControlCommand.kd_cartesian[1] << ", " << legControlCommand.kd_cartesian[2]<< ", " << legControlCommand.kd_cartesian[3] << ", " << legControlCommand.kd_cartesian[4] << ", " << legControlCommand.kd_cartesian[5] << ", ";
            outputFile << legControlCommand.kd_cartesian[6] << ", " << legControlCommand.kd_cartesian[7] << ", " << legControlCommand.kd_cartesian[8]<< ", " << legControlCommand.kd_cartesian[9] << ", " << legControlCommand.kd_cartesian[10] << ", " << legControlCommand.kd_cartesian[11] << "\n";
            outputFile << "legControlCommand.kp_joint: " << legControlCommand.kp_joint[0] << ", " << legControlCommand.kp_joint[1] << ", " << legControlCommand.kp_joint[2]<< ", " << legControlCommand.kp_joint[3] << ", " << legControlCommand.kp_joint[4] << ", " << legControlCommand.kp_joint[5] << ", ";
            outputFile << legControlCommand.kp_joint[6] << ", " << legControlCommand.kp_joint[7] << ", " << legControlCommand.kp_joint[8]<< ", " << legControlCommand.kp_joint[9] << ", " << legControlCommand.kp_joint[10] << ", " << legControlCommand.kp_joint[11] << "\n";
            outputFile << "legControlCommand.kd_joint: " << legControlCommand.kd_joint[0] << ", " << legControlCommand.kd_joint[1] << ", " << legControlCommand.kd_joint[2]<< ", " << legControlCommand.kd_joint[3] << ", " << legControlCommand.kd_joint[4] << ", " << legControlCommand.kd_joint[5] << ", ";
            outputFile << legControlCommand.kd_joint[6] << ", " << legControlCommand.kd_joint[7] << ", " << legControlCommand.kd_joint[8]<< ", " << legControlCommand.kd_joint[9] << ", " << legControlCommand.kd_joint[10] << ", " << legControlCommand.kd_joint[11] << "\n";

            outputFile << "udpData.p_des: " << udpData.p_des[0] << ", " << udpData.p_des[1] << "\n";
            outputFile << "udpData.v_des: " << udpData.v_des[0] << ", " << udpData.v_des[1] << ", " << udpData.v_des[2] << "\n";
            outputFile << "udpData.rpy_des: " << udpData.rpy_des[0] << ", " << udpData.rpy_des[1] << ", " << udpData.rpy_des[2] << "\n";
            outputFile << "udpData.omega_des: " << udpData.omega_des[0] << ", " << udpData.omega_des[1] << ", " << udpData.omega_des[2] << "\n";
            outputFile.close();
            LOG_INFO("Data has been written to lcm_data.txt");
        } else {
            LOG_ERROR("Failed to open the file for writing.");
        }
    }
}

float  CMessageLcmHandler::GetControlCmdStatus(){
    float  tau = 0.0;
    for(int i =0; i <12 ; ++i){
        tau +=  legControlCommand.tau_ff[i];
    }
    return tau;
}

float  CMessageLcmHandler::GetPowerStatus(){
    float  power = 0.0;
    power = stateEstData.power/1000;
    return power;
}